import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;


public class RobotHardware {

	public UltrasonicSensor us;
	public LightSensor ls;
	public TouchSensor tsLeft;
	public TouchSensor tsRight;
	public DifferentialPilot pilot;

	final float WHEEL_DIAM = 5.6F;
	final float TRACK_W = 17.7F;
	
	public RobotHardware(){
		us = new UltrasonicSensor(SensorPort.S4);
		ls = new LightSensor(SensorPort.S1);
		tsLeft = new TouchSensor(SensorPort.S2);
		tsRight = new TouchSensor(SensorPort.S3);
		pilot = new DifferentialPilot(WHEEL_DIAM, TRACK_W, Motor.C, Motor.A);
		pilot.setRotateSpeed(50);
		pilot.setTravelSpeed(20);
		pilot.setAcceleration(30);
	}
	
	public double readUltrasonic(){
		return us.getDistance();
	}
	
	public int readLight(){
		return ls.getNormalizedLightValue();
	}
	
	public void rotate(double angle){
		pilot.rotate(angle);
	}
	
	public void goForward(){
		pilot.travel(Constants.GRID_SIZE + 1);
	}

	
	public void climbUp(){
		Motor.B.rotateTo(90);
		pilot.setTravelSpeed(20);
		while(!(tsLeft.isPressed() || tsRight.isPressed())){
			int distance = (int) readUltrasonic();
			LCD.drawInt(distance, 3, 4);
			if(distance <= 16)
				pilot.arcForward(3000);
			else
				pilot.arcForward(-3000);
		}
		Motor.B.rotateTo(0);
		pilot.stop();
		pilot.travel(-10);
		pilot.setTravelSpeed(20);
	}	
	
	public void climbDown(){
		Motor.B.rotateTo(180);
		pilot.setTravelSpeed(5);
		pilot.travel(-30);
		while(readUltrasonic() > 12){
			Motor.B.rotateTo(90);
			int distance = (int) readUltrasonic();
			LCD.drawInt(distance, 3, 4);
			if(distance <= 16)
				pilot.arcBackward(3000);
			else
				pilot.arcBackward(-3000);
			Motor.B.rotateTo(180);
		}
		Motor.B.rotateTo(0);
		pilot.stop();
		pilot.setTravelSpeed(20);
	}
	
	public void correction(int bias){
		pilot.setTravelSpeed(10);
		boolean leftPressed = false, rightPressed = false;
		while(!leftPressed && !rightPressed){
			pilot.travel(20, true);
			leftPressed = tsLeft.isPressed();
			rightPressed = tsRight.isPressed();
		}
		pilot.stop();
		pilot.setTravelSpeed(4);
		if(!rightPressed){
			pilot.rotate(-90, true);
			while(!tsRight.isPressed()){}
		}
		else if(!leftPressed){
			pilot.rotate(90, true);
			while(!tsLeft.isPressed()){}
		}
		pilot.stop();
		while(!(leftPressed && rightPressed)){
			pilot.travel(20, true);
			leftPressed = tsLeft.isPressed();
			rightPressed = tsRight.isPressed();
		}
		pilot.stop();
		pilot.setTravelSpeed(10);
		pilot.travel(-8 + bias);
		pilot.setTravelSpeed(20);
	}
}
